#include "status.h"

uint8_t flag_OnStage = 1;    //在台上标志   默认为1
uint8_t flag_object = 0;     //有物体标志   默认为0
uint8_t flag_UnderStage = 0; //在台下标志   默认为0
uint8_t flag_backStage = 0;

uint8_t OU_verdict()
{
  uint8_t pose = pswDetect(UP);
  
  pose = IF(pose, 1) + IF(pose, 2) + IF(pose, 3) + IF(pose, 4);
  
  if(pose == 4)
  return 1;
  else 
  return 0;
}
uint8_t isOnStage()
{
    uint8_t pose = 0;
    uint8_t tmp = 0;

    flag_OnStage = 0;
    pose = pswDetect(UP);
    if (pose == 0 || pose == 0b0001 || pose == 0b0010) //无姿态记录
    {
            flag_OnStage = 1; //此时说明在台上
    }
    else //滤波
    {
      delay(10);
      tmp = pswDetect(UP);
      if (tmp != pose)
        pose = 0;
    }
    
    return pose;
}

uint8_t isUnderStage()
{
    uint8_t psw = pswDetect(DOWN);
    uint8_t dist = distanceDetect(DISTANCE);
    //前方红外测距和光电传感器 ：1
    //后方红外测距和光电传感器 ：2
    //左方红外测距和光电传感器 ：3
    //右方红外测距和光电传感器 ：4

    /*************** 一个红外测距模块检测到 ******************/
    //后方朝向擂台
    if(IF(psw,1) == 0 && IF(psw,2) == 1 && IF(psw,3) == 0 && IF(psw,4) == 0
    && IF(dist,1) == 1 && IF(dist,2) == 0 && IF(dist,3) == 0 && IF(dist,4) == 0)
            return 1;
    //左方朝向擂台
    if(IF(psw,1) == 0 && IF(psw,2) == 0 && IF(psw,3) == 1 && IF(psw,4) == 0
    && IF(dist,1) == 0 && IF(dist,2) == 0 && IF(dist,3) == 0 && IF(dist,4) == 1)
            return 2;
    //右方朝向擂台
    if(IF(psw,1) == 0 && IF(psw,2) == 0 && IF(psw,3) == 0 && IF(psw,4) == 1
    && IF(dist,1) == 0 && IF(dist,2) == 0 && IF(dist,3) == 1 && IF(dist,4) == 0)
            return 3;
    //前方朝向擂台
    if(IF(psw,1) == 1 && IF(psw,2) == 0 && IF(psw,3) == 0 && IF(psw,4) == 0
    && IF(dist,1) == 0 && IF(dist,2) == 1 && IF(dist,3) == 0 && IF(dist,4) == 0)
            return 4;
   
    /************** 两个相邻红外测距模块检测到 ***************/
    //前左检测到围栏
    if(IF(psw,1) == 0 && IF(psw,3) == 0 
    && IF(dist,1) == 1 && IF(dist,2) == 0 && IF(dist,3) == 1 && IF(dist,4) == 0)
            return 5;  
    //前右检测到围栏
    if(IF(psw,1) == 0 && IF(psw,4) == 0 
    && IF(dist,1) == 1 && IF(dist,2) == 0 && IF(dist,3) == 0 && IF(dist,4) == 1)
            return 6;
    //后左检测到围栏
    if(IF(psw,2) == 0 && IF(psw,3) == 0 
    && IF(dist,1) == 0 && IF(dist,2) == 1 && IF(dist,3) == 1 && IF(dist,4) == 0)
            return 7;  
    //后右检测到围栏
    if(IF(psw,2) == 0 && IF(psw,4) == 0 
    && IF(dist,1) == 0 && IF(dist,2) == 1 && IF(dist,3) == 0 && IF(dist,4) == 1)
            return 8;        
    
    /************* 台上有敌人，两个相对测距检测到**************/
    //前方或后方有台上敌人
    if(IF(dist,1) == 1 && IF(dist,2) == 1 && IF(dist,3) == 0 && IF(dist,4) == 0)
            return 9; 
    //左方或右方有台上敌人
    if(IF(dist,1) == 0 && IF(dist,2) == 0 && IF(dist,3) == 1 && IF(dist,4) == 1)
            return 10; 
    
    /************* 三侧有障碍，三个测距检测到 ****************/
    //前方、左侧和右侧检测到围栏
    if(IF(dist,1) == 1 && IF(dist,2) == 0 && IF(dist,3) == 1 && IF(dist,4) == 1)
            return 11;
    //前方、右侧和后方检测到围栏
    if(IF(dist,1) == 1 && IF(dist,2) == 1 && IF(dist,3) == 0 && IF(dist,4) == 1)
            return 12;
    //前方、左侧和后方检测到围栏
    if(IF(dist,1) == 1 && IF(dist,2) == 1 && IF(dist,3) == 1 && IF(dist,4) == 0)
            return 13;
    //右侧、左侧和后方检测到围栏
    if(IF(dist,1) == 0 && IF(dist,2) == 1 && IF(dist,3) == 1 && IF(dist,4) == 1)
            return 14;

    /************* 斜对擂台，两个光电传感器检测到 ************/
    //前方和右侧对擂台，其他传感器没检测到
    if(IF(psw,1) == 1 && IF(psw,4) == 1
    && IF(dist,1) == 0 && IF(dist,4) == 0)
            return 15;
    //前方和左侧对擂台，其他传感器没检测到
    if(IF(psw,1) == 1 && IF(psw,3) == 1
    && IF(dist,1) == 0 && IF(dist,3) == 0)
            return 16;
    //后方和右侧对擂台，其他传感器没检测到
    if(IF(psw,2) == 1 && IF(psw,4) == 1
    && IF(dist,2) == 0 && IF(dist,4) == 0)
            return 17;
    //后方和左侧对擂台，其他传感器没检测到
    if(IF(psw,2) == 1 && IF(psw,3) == 1
    && IF(dist,2) == 0 && IF(dist,3) == 0)
            return 18;
    else 
            return NONE_toStage;
}

uint8_t isObject()
{
    uint8_t direction = 0;
    uint8_t tmp = 0;

    flag_object = 1;
    direction = pswDetect(DOWN);

    if (direction == 0) //无姿态记录
    {
        flag_object = 0;
    }
    else
    {
        delay(10); //滤波
        tmp = pswDetect(DOWN);
        if (tmp != direction)
            direction = 0;
    }

    return direction;
}

/**
   @brief 得到psw的值
   @param uint8_t upOrDown: UP DOWN
   @return uint8_t
 * */
uint8_t pswDetect(uint8_t upOrDown)
{
    uint8_t psw[4] = {0};
    uint8_t pswDirection = 0;

    if (upOrDown == UP)
        getPsw(psw, 0b1111);
    else if (upOrDown == DOWN)
        getPsw(psw, 0b11110000);

    for (int i = 0; i < 4; i++)
    {
        pswDirection |= psw[i] << i;
    }

    if (upOrDown == DOWN)
        pswDirection = ~pswDirection & 0x0F;

    return pswDirection;
}
/**
   @brief 得到distance的值
   @param int range
   @return uint8_t
 * */
uint8_t distanceDetect(int range)
{
    int distance[4] = {0};
    getDistance(distance, 0x0F);

    uint8_t distanceDirection = 0;

    for (int i = 0; i < 4; i++)
    {
        if (distance[i] > range)
        {
            distanceDirection |= (1 << i);
        }
    }

    return distanceDirection;
}
